
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       pid_basic.h
  * @author     baiyang
  * @date       2021-8-29
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include <stdbool.h>
#include <stdint.h>
#include <common/gp_math/gp_mathlib.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/
typedef struct
{
    // parameters
    float _kp;
    float _ki;
    float _kd;
    float _kff;
    float _kimax;
    float _filt_E_hz;         // PID error filter frequency in Hz
    float _filt_D_hz;         // PID derivative filter frequency in Hz

    // internal variables
    float _dt;          // timestep in seconds
    float _target;      // target value to enable filtering
    float _error;       // error value to enable filtering
    float _derivative;  // last derivative for low-pass filter
    float _integrator;  // integrator value
    bool _reset_filter; // true when input filter should be reset during next call to set_input
} PID_basic_ctrl;

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
// Constructor for PID
void pid_basic_ctrl_ctor(PID_basic_ctrl * controler, float initial_p, float initial_i, float initial_d, float initial_ff, float initial_imax, float initial_filt_E_hz, float initial_filt_D_hz, float dt);

// set time step in seconds
static inline void pid_basic_ctrl_set_dt(PID_basic_ctrl * controler, float dt) { controler->_dt = dt; }

// set target and measured inputs to PID controller and calculate outputs
// target and error are filtered
// the derivative is then calculated and filtered
// the integral is then updated based on the setting of the limit flag
float pid_basic_ctrl_update_all(PID_basic_ctrl * controler, float target, float measurement, bool limit);
float pid_basic_ctrl_update_all2(PID_basic_ctrl * controler, float target, float measurement, bool limit_neg, bool limit_pos);

// update the integral
// if the limit flags are set the integral is only allowed to shrink
void pid_basic_ctrl_update_i(PID_basic_ctrl * controler, bool limit_neg, bool limit_pos);

// get results from pid controller
static inline float pid_basic_ctrl_get_p(PID_basic_ctrl * controler) { return controler->_error * controler->_kp; }
static inline float pid_basic_ctrl_get_i(PID_basic_ctrl * controler) { return controler->_integrator; }
static inline float pid_basic_ctrl_get_d(PID_basic_ctrl * controler) { return controler->_derivative * controler->_kd; }
static inline float pid_basic_ctrl_get_ff(PID_basic_ctrl * controler) { return controler->_target * controler->_kff; }
static inline float pid_basic_ctrl_get_error(PID_basic_ctrl * controler) { return controler->_error; }

// reset the integrator
static inline void pid_basic_ctrl_reset_I(PID_basic_ctrl * controler) { controler->_integrator = 0.0f; }

// input and D term filter will be reset to the next value provided to set_input()
static inline void pid_basic_ctrl_reset_filter(PID_basic_ctrl * controler) { controler->_reset_filter = true; }

// save gain to eeprom
void save_gains();

// get accessors
static inline float pid_basic_ctrl_get_kP(PID_basic_ctrl * controler) { return controler->_kp; }
static inline float pid_basic_ctrl_get_kI(PID_basic_ctrl * controler) { return controler->_ki; }
static inline float pid_basic_ctrl_get_kD(PID_basic_ctrl * controler) { return controler->_kd; }
static inline float pid_basic_ctrl_get_kff(PID_basic_ctrl * controler) { return controler->_kff;}
static inline float pid_basic_ctrl_get_filt_E_hz(PID_basic_ctrl * controler) { return controler->_filt_E_hz; }
static inline float pid_basic_ctrl_get_filt_D_hz(PID_basic_ctrl * controler) { return controler->_filt_D_hz; }
static inline float pid_basic_ctrl_get_imax(PID_basic_ctrl * controler) { return controler->_kimax; }
float pid_basic_ctrl_get_filt_E_alpha(PID_basic_ctrl * controler);
float pid_basic_ctrl_get_filt_D_alpha(PID_basic_ctrl * controler);

// set accessors
static inline void pid_basic_ctrl_set_kP(PID_basic_ctrl * controler, float v) { controler->_kp = v; }
static inline void pid_basic_ctrl_set_kI(PID_basic_ctrl * controler, float v) { controler->_ki = v; }
static inline void pid_basic_ctrl_set_kD(PID_basic_ctrl * controler, float v) { controler->_kd = v; }
static inline void pid_basic_ctrl_set_kff(PID_basic_ctrl * controler, float v) { controler->_kff = v; }
static inline void pid_basic_ctrl_set_imax(PID_basic_ctrl * controler, float v) { controler->_kimax = fabsf(v); }
static inline void pid_basic_ctrl_set_filt_E_hz(PID_basic_ctrl * controler, float hz) { controler->_filt_E_hz = fabsf(hz); }
static inline void pid_basic_ctrl_set_filt_D_hz(PID_basic_ctrl * controler, float hz) { controler->_filt_D_hz = fabsf(hz); }

// integrator setting functions
void pid_basic_ctrl_set_integrator3(PID_basic_ctrl * controler, float target, float measurement, float i);
void pid_basic_ctrl_set_integrator2(PID_basic_ctrl * controler, float error, float i);
void pid_basic_ctrl_set_integrator(PID_basic_ctrl * controler, float i);

/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



